#include <motor.h>
#include <inttypes.h>
#include <stm32l1xx.h>
#include <arm_math.h>

int32_t /*phi_des,phi_act,*/omega_newdes,omega_des,omega_act/*,accel_des*/; // ziadany uhol natocenia, skutocny uhol natocenia, ziadane otacky, okamzite otacky, ziadana akceleracia (rampy)
uint8_t mode = MODE_SPEED;
uint32_t ireg_i,ireg_p; // parametre regulatora prudu, ireg_i = Ki/Tvz
int32_t i_des,i_act; // ziadana hodnota prudu, skutocna hodnota prudu

int8_t ramp_on = 0;
int8_t rampnum;
uint8_t idir = 1; // smer prudu, znamienko

arm_pid_instance_q15 regulator_prudu,regulator_rychlosti;

void iREGinit(void);
void wREGinit(void);

void motor_init(void) {
	TIM2 -> PSC = 0; 
	TIM2 -> ARR = 767; // 16 MHz/768 = 20,8 kHz
	TIM2 -> CCR3 = 256;
	TIM2 -> CCMR2 |= TIM_CCMR2_OC3FE | TIM_CCMR2_OC3PE | TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3M_2;
	TIM2 -> CCER |= TIM_CCER_CC3E;
	TIM2 -> CR1 |= TIM_CR1_CEN; // enable TIMER 3
	omega_newdes = omega_des = 0;
	iREGinit();
	wREGinit();
	ramp_on = 0;
	rampnum = 1;
}

void iREG(uint16_t *convres) {
	int32_t eact;

	int16_t u;
	int32_t toccr;

	i_act = ((convres[0]+convres[2]+convres[4]+convres[6]) / 4) >> 2; // aritmeticky priemer, jeden posun pre znamienko, jeden pre absenciu zosilnovaca (R56 miesto 1R2 ako bocnik)
	eact = i_des - (((int16_t) i_act)*idir);

	u = arm_pid_q15(&regulator_prudu,eact);

	u = (u > 0 ? u : 0); // iba 'kladny prud'
	toccr = (u*3) >> 6;     // 3*u/64
	idir = 1;
//	toccr = ((u > 0 ? u : -u)*3) >> 6;     // 3*u/64
//	if (u > 0) {	// prepinanie polarity na L298, PB11/12 zaviazane na vstupy, PB10 ako PWM na EN
	GPIOB -> ODR |=  (0x0800);
	GPIOB -> ODR &= ~(0x1000);
//	} else {
//		if (u != 0) {
//			idir = -1;
//			GPIOB -> ODR &= ~(0x0800);
//			GPIOB -> ODR |=  (0x1000);
//		}
//		else {
//			GPIOB -> ODR &= ~(0x1800);
//		}
//	}
	TIM2 -> CCR3 = toccr;
	GPIOC -> ODR ^= 1;
}


int flag;
int32_t fi,fi_prev;

void wREG(void) {
	fi = TIM3 -> CNT;
	omega_act = (fi - fi_prev); // doriesit pretecenie
	fi_prev = fi;

	if (omega_act > (1 << 12))
		omega_act = omega_act - 65536;
	if (omega_act < (-(1 << 12)))
		omega_act = omega_act + 65536;
	omega_act = omega_act * 100; // vzorkovacia frekvencia

	i_des = arm_pid_q15(&regulator_rychlosti,omega_des - omega_act);
	if (i_des < 0)
		i_des = 0;
	flag++;
	GPIOC -> ODR ^= 2;

	if (omega_des != omega_newdes) {
		if (ramp_on) {
			if (omega_des > omega_newdes) {
				omega_des -= rampnum;
				if (omega_des < omega_newdes)
					omega_des = omega_newdes;
			}
			else
				omega_des += rampnum;
				if (omega_des > omega_newdes)
					omega_des = omega_newdes;
		} else {
			omega_des = omega_newdes;
		}
	}
}

void iREGinit(void) {
// nastavenia pre Tvz=100us => 10 kHz
	regulator_prudu.Kp =    29*10; // 0.2256453 >> 8 => 0.000881427 << 16 =>    28.8   =>    29
	regulator_prudu.Kd =     0;
	regulator_prudu.Ki = 26847*1; // 209.7447 >> 8  => 0.81931523  << 16 => 26847.232 => 26847
	arm_pid_init_q15(&regulator_prudu,1);
}

void wREGinit(void) {
// nastavenia pre 100 Hz, Cu = 8.3e-3 Vs
	regulator_rychlosti.Kp = 329*60; // Kp/Cu = 0.0100 << 16 => 329.2 => 329
	regulator_rychlosti.Kd = 0;
	regulator_rychlosti.Ki = 18*50;   // Ki/Cu = 5.5779e-4 << 16 => 18.3 => 18
	arm_pid_init_q15(&regulator_rychlosti,1);
}

